//
// Created by sun on 2020/5/28.
//
#ifndef SRC_DRONE_UNIT_NEW_H
#define SRC_DRONE_UNIT_NEW_H

#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <ctime>
#include <geometry_msgs/PoseStamped.h>
#include "px4_gcs/GCSMessage.h"
#include "tf/transform_datatypes.h"
using namespace std;
#define CORRECT_NUM 100


class OffboardControler{

public:
    OffboardControler();
    ros::NodeHandle ros_nodehandle;

    ros::Publisher de_pos_sp_pub{};
    ros::Publisher drone_pos_pub{};
    ros::Subscriber drone_pos_sub{};
    ros::Subscriber out_sp_sub{};

    geometry_msgs::PoseStamped backed_pose;

    float start_angle[3]{0,0,0};
    float cur_pos[3]{0,0,0};
    float sp_pos[3]{0,0,0};
    float sp_rpyt[4]{0,0,0,0};
    float cur_angle[3]{0,0,0};
    int correct_count = 0;
    tf::Quaternion cur_q{};
    string node_name;
    serial::Serial cur_ser{};
    string drone_name;
    MODE drone_mode[3] = {GPS,VICON,CAMERA};
    SEND_CURRENT_STATE drone_state[3] = {SEND_NED_POSITION,SEND_NED_POSITION_RPY,SEND_NED_RPY};
    SETPOINT_TYPE drone_sp[4] = {LOCAL_POSITION_SP,GLOBAL_POSITION_SP,ATTITUDE_SP,VELOCITY_SP};

    template <typename T>
    T readParam(ros::NodeHandle &n, std::string name);

    void getAllParam(ros::NodeHandle &n);

    bool initSerialCom();

    friend void out_sp_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);
    friend void drone_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);

    int id{0};
    int current_mode{0};
    int current_sp{0};
    int current_state{0};
    int baudrate{57600};
    int serial_timeout{10};
    int rate{30};
    float takeoff_height{0.5};
    float init_pose_x{0};
    float init_pose_y{0};
    string sub_pos_topic_name{"/drone0/vision/pose"};
    int use_out_sp{1} ;
    string out_sp_name{"/drone0/sp"};
    string serial_name{"/dev/ttyUSB0"};

    GCSMessageManager gcsMessageManager;
    CMDMessage cmd_msg;
    CSTMessage cst_msg;
    SPMessage sp_msg;
    UAVMessage recv_msg;
};

#endif //SRC_DRONE_UNIT_NEW_H
